#include <cstdlib>
#include <iostream>
#include <libobsensor/hpp/Error.hpp>
#include <memory>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
 
#include "camera.h"
#include "myaruco.h"

int main(void) {
    try {
        Camera camera(true, false, false);
        camera.start();

        cv::Mat cameraMatrix, distCoeffs;
        myaruco::readCameraParameters(cameraMatrix, distCoeffs);
        cv::Ptr<cv::aruco::Dictionary> dictionary;
        myaruco::maker_init_dic(dictionary, cv::aruco::DICT_5X5_50);
        // myaruco::maker_init_dic(dictionary, cv::aruco::DICT_6X6_250);

        while (cv::waitKey(1) != 27) { // ESC的ASCII码是27，按ESC键可以终止程序
            auto frame_set = camera.get();
            if (frame_set != nullptr) {
                cv::Mat img = camera.frame2mat(frame_set->colorFrame());
                if (!img.empty()) {
                    cv::imshow("Color", img);
                    myaruco::maker_detect_video(img, dictionary);
                    // myaruco::maker_pose_estimation(img, dictionary, cameraMatrix, distCoeffs);
                }
                img = camera.frame2mat(frame_set->irFrame());
                if (!img.empty()) {
                    cv::imshow("IR", img);
                }
                img = camera.frame2mat(frame_set->depthFrame());
                if (!img.empty()) {
                    cv::imshow("Depth", img);
                }
            }
        }
        camera.stop();
        cv::destroyAllWindows();
    } catch (const ob::Error &e) {
        std::cerr << "Function:" << e.getName() << "\nargs:" << e.getArgs()
                  << "\nmessage:" << e.getMessage() << "\ntype:" << e.getExceptionType()
                  << std::endl;
        exit(EXIT_FAILURE);
    } catch (const std::exception &e) {
        std::cerr << e.what() << std::endl;
        exit(EXIT_FAILURE);
    } catch (...) {
        std::cerr << "Unexpected Error!" << std::endl;
        exit(EXIT_FAILURE);
    }
    return EXIT_SUCCESS;
}